#include <QtGui>

#include <QDebug>

#include "animatorwidget.h"
#include "animationlabel.h"
#include "glwidget.h"
#include "robot.h"
#include "core.h"

animatorWidget::animatorWidget(Robot *robot, Core *core)
{
	robotPointer = robot;
	corePointer = core;
	
	timeout = 40;	//40ms = 25 fps
	
	glWidget = new GLWidget(robotPointer);
	connect(corePointer, SIGNAL(robotUpdated()), glWidget,SLOT(updateWidget()));
	connect(corePointer, SIGNAL(robotLoaded()), glWidget, SLOT(robotLoaded()));
	
	stopButton = new QPushButton(QString("STOP"));
	playButton = new QPushButton(QString("PLAY"));
	nextButton = new QPushButton(QString("NEXT"));
	prevButton = new QPushButton(QString("PREV"));
	loopButton = new QPushButton(QString("LOOP"));
	stepButton = new QPushButton(QString(">>>>"));
	firstButton = new QPushButton(QString("FIRST"));
	lastButton = new QPushButton(QString("LAST"));
	
	connect(playButton, SIGNAL(clicked()), this, SLOT(playAnimation()));
	connect(stopButton, SIGNAL(clicked()), this, SLOT(stopAnimation()));
	connect(nextButton, SIGNAL(clicked()), this, SLOT(nextAnimation()));
	connect(prevButton, SIGNAL(clicked()), this, SLOT(prevAnimation()));
	connect(firstButton, SIGNAL(clicked()), this, SLOT(firstAnimation()));
	connect(lastButton, SIGNAL(clicked()), this, SLOT(lastAnimation()));
	connect(stepButton, SIGNAL(clicked()), this, SLOT(stepAnimations()));
	connect(loopButton, SIGNAL(clicked()), this, SLOT(loopAnimations()));
	
	speedSlider = new QSlider(Qt::Horizontal);
	speedSlider->setTickPosition(QSlider::TicksBelow);
	speedSlider->setRange(0, 100);
	speedSlider->setSingleStep(1);
	speedSlider->setPageStep(10);
	speedSlider->setSliderPosition(50);
	
	sliderLabel = new QLabel(QString("Animation Speed"));
	
	mainLayout = new QGridLayout;
	mainLayout->addWidget(glWidget, 0, 0, 10, 6);
	mainLayout->addWidget(firstButton, 10, 0);
	mainLayout->addWidget(prevButton, 10, 1);
	mainLayout->addWidget(playButton, 10, 2);
	mainLayout->addWidget(stopButton, 10, 3);
	mainLayout->addWidget(nextButton, 10, 4);
	mainLayout->addWidget(lastButton, 10, 5);
	mainLayout->addWidget(stepButton, 10, 6);
	mainLayout->addWidget(loopButton, 11, 6); 
	mainLayout->addWidget(speedSlider, 10, 7);
	mainLayout->addWidget(sliderLabel, 11, 7);
	
	this->setLayout(mainLayout);
	
	timer = new QTimer(this);
	connect(timer, SIGNAL(timeout()), this, SLOT(animationTick()));
	
	connect(this, SIGNAL(changePlayPriority(int)), this, SLOT(playPriorityChanged(int)));
	
	mode = 0;
	
	tabIsOpen = false;
	
	//this is stupid
	stepThruAnimations = true;
	loopAllAnimations = true;
	stepAnimations();
	loopAnimations();
	
	generateAnimationLabels();
	emit changePlayPriority(0);
}

void animatorWidget::setTabIndex(int index)
{
	tabIndex = index;
}

void animatorWidget::robotLoaded()	//SLOT
{
	generateAnimationLabels();	//could have saved the function call, but this is better organized
	
	pushJoints();	//save new joints
}

void animatorWidget::generateAnimationLabels()
{
	for (int i=0; i < animationLabelList.size(); i++)
	{
		delete animationLabelList[i];
	}
	
	animationLabelList.clear();
	
	for (int i=0; i<robotPointer->animationsList.size(); i++)
	{
		aniLabel = new AnimationLabel(robotPointer->animationsList[i]->jointNumber(), robotPointer->jointList[robotPointer->animationsList[i]->jointNumber()].getType(),
					robotPointer->animationsList[i]->startValue(),
					robotPointer->animationsList[i]->stopValue(),
					robotPointer->animationsList[i]->priority());
		
		animationLabelList.append(aniLabel);
		
		connect(this, SIGNAL(changePlayPriority(int)), animationLabelList[i], SLOT(playing(int)));
		connect(robotPointer->animationsList[i], SIGNAL(ticked(float)), animationLabelList[i], SLOT(setValue(float)));
	}

	for (int i=0; i< animationLabelList.size(); i++)
	{
		mainLayout->addWidget(animationLabelList[i], i, 7);
	}
	
	firstAnimation();
}

void animatorWidget::tabSelected(int index)
{
	
	if (index == tabIndex)
	{
		pushJoints();	//save all joint values
		canPlay = true;
		emit changePlayPriority(0);
		initializeJoints();
		tabIsOpen = true;
		qDebug()<<"animatorWidget selected";
	}
	else
	{	
		if (tabIsOpen == true) 
		{	
			popJoints();	//reset all joint values
			canPlay = false;
			mode = 0;		//stop
			emit changePlayPriority(-1);
			timer->stop();
			tabIsOpen = false;
		qDebug()<<"another widget selected";
		}
	}
}

void animatorWidget::animationTick()
{
	if (mode == 2)	//only tick while playing
	{
		for (int i=0; i< robotPointer->animationsList.size(); i++)
		{
			if (robotPointer->animationsList[i]->priority() == currentPlayPriority)
			{	
				switch (robotPointer->jointList[robotPointer->animationsList[i]->jointNumber()].getType())
				{
				case 1:	//revolute
					//move at max rate of 180 degrees/sec
					robotPointer->animationsList[i]->tick((float(speedSlider->value())/100.0)*4.5);
					corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->currentValue());
					break;
				case 2: //prismatic
					//move at max rate of 0.05m /tick at max speed (i.e. 2m/s)
					robotPointer->animationsList[i]->tick((float(speedSlider->value())/100.0)*0.05);
					corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->currentValue());
					break;
				}
			}
		}
		
		int animationsAtPriority;
		int animationsDone;
		
		animationsAtPriority = 0;
		animationsDone = 0;
		
		//check if all joints in current priority have finished movements
		for (int i=0; i< robotPointer->animationsList.size(); i++)
		{
			if (robotPointer->animationsList[i]->priority() == currentPlayPriority)
			{
				animationsAtPriority++;
				
				if (robotPointer->animationsList[i]->atEnd())
					animationsDone++;
			}
		}
		
		if (animationsAtPriority == animationsDone)
		{
			if ((robotPointer->maxAnimationPriority == currentPlayPriority) && (loopAllAnimations) && (!stepThruAnimations))
			{
				emit changePlayPriority(0);
				initializeJoints();
			}
			else if ((robotPointer->maxAnimationPriority == currentPlayPriority) && (!loopAllAnimations))
			{
				mode = 0; 	//stop playing and reset next time around
				emit changePlayPriority(0);
				timer->stop();
				playButton->setText(QString("PLAY"));
			}
				
			else if ((robotPointer->maxAnimationPriority > currentPlayPriority) && (!stepThruAnimations))
				emit changePlayPriority(++currentPlayPriority);
		}
	}
}

void animatorWidget::initializeJoints()
{
	//set joints to start positions
	for (int i=0; i<robotPointer->animationsList.size(); i++)
	{
		robotPointer->animationsList[i]->start();
		corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->startValue());
	}
}

void animatorWidget::playPriorityChanged(int priority)
{
	currentPlayPriority = priority;
// 	qDebug()<<"Priority changed to"<<priority;
}

void animatorWidget::pushJoints()
{
	jointValuesList.clear();
	
	for (int i=0; i<robotPointer->jointList.size(); i++)
	{
		jointValuesList.append(robotPointer->jointList[i].getCurrentActuation());
	}
	
}

void animatorWidget::popJoints()
{
	for (int i=0; i<jointValuesList.size(); i++)
	{
		corePointer->adjustJoint(i, jointValuesList.at(i));
	}
	
	//set animations to start again
	for (int i=0; i< robotPointer->animationsList.size(); i++)
	{
		robotPointer->animationsList[i]->start();
	}
}

void animatorWidget::stopAnimation()
{
	mode = 1; 
	for (int i=0; i< robotPointer->animationsList.size(); i++)
	{
		if (robotPointer->animationsList[i]->priority() <= currentPlayPriority)
		{
			robotPointer->animationsList[i]->start();
		}
	}
	playButton->setText(QString("PLAY"));
	
	timer->stop();
	
	for (int i=0; i<robotPointer->animationsList.size(); i++)
	{
		if (robotPointer->animationsList[i]->priority() < currentPlayPriority)
		{
			robotPointer->animationsList[i]->end();
			corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->stopValue());
		}
		else
		{	
			robotPointer->animationsList[i]->start();
			corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->startValue());
		}
	}
}

void animatorWidget::playAnimation()
{
	if (mode == 2) 
	{
		mode = 3;	//pause if we are already playing
		playButton->setText(QString("PLAY"));
	}	
	else
	{	
		if (mode == 0)	//were reset (i.e. reached end)
			initializeJoints();
			
		mode = 2;	//start playing
		timer->start(timeout);
		playButton->setText(QString("PAUSE"));
		if (currentPlayPriority == -1)
			emit changePlayPriority(0);
	}
}

void animatorWidget::nextAnimation()
{
	for (int i=0; i<robotPointer->animationsList.size(); i++)
	{
		if (robotPointer->animationsList[i]->priority() <= currentPlayPriority)
		{
			robotPointer->animationsList[i]->end();
			corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->stopValue());
		}
	}
	
	if (robotPointer->maxAnimationPriority > currentPlayPriority)
	{
		emit changePlayPriority(++currentPlayPriority);
	}
	else if (robotPointer->maxAnimationPriority == currentPlayPriority)
	{
// 		currentPlayPriority = 0;
		mode = 0;
		playButton->setText(QString("PLAY"));
		timer->stop();
		qDebug()<<"Stopped by nextAnimations";
	}
}

void animatorWidget::prevAnimation()
{
		qDebug()<<"currentPlayPriority"<<currentPlayPriority<<"mode is"<<mode;
	
	if (currentPlayPriority > 0)
		emit changePlayPriority(--currentPlayPriority);
	else if (currentPlayPriority < 0)
		emit changePlayPriority(0);
// 	else if (currentPlayPriority == 0)
// 		currentPlayPriority = robotPointer->maxAnimationPriority;
	
	for (int i=0; i<robotPointer->animationsList.size(); i++)
	{
		if (robotPointer->animationsList[i]->priority() < currentPlayPriority)
		{
			robotPointer->animationsList[i]->end();
			corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->stopValue());
		}
		else
		{
			robotPointer->animationsList[i]->start();
			corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->startValue());
		}
	}
}

void animatorWidget::loopAnimations()
{
	if (loopAllAnimations)
	{
		loopButton->setText(QString("~LOOP"));
	}
	else
	{
		loopButton->setText(QString("LOOP"));
		
	}
	loopAllAnimations ^= true;	//toggle
}

void animatorWidget::stepAnimations()
{
	if (!stepThruAnimations)
	{
		stepButton->setText(QString(">|>|"));
	}
	else
	{
		stepButton->setText(QString(">>>>"));
	}
	stepThruAnimations ^= true;	//toggle
}

void animatorWidget::firstAnimation()
{
	emit changePlayPriority(0);
	
	for (int i=0; i<robotPointer->animationsList.size(); i++)
	{
		robotPointer->animationsList[i]->start();
		corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->startValue());
	}
}

void animatorWidget::lastAnimation()
{
	emit changePlayPriority(robotPointer->maxAnimationPriority);
	mode = 1; //we have stopped at start of last animation

	for (int i=0; i<robotPointer->animationsList.size(); i++)
	{
		if (robotPointer->animationsList[i]->priority() < currentPlayPriority)
		{
			robotPointer->animationsList[i]->end();
			corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->stopValue());
		}
		else
		{
			robotPointer->animationsList[i]->start();
			corePointer->adjustJoint(robotPointer->animationsList[i]->jointNumber(), robotPointer->animationsList[i]->startValue());
		}
	}
}
